package it.gius.pe.algorithm.gjk.shapes;

import org.jbox2d.common.Vec2;

import it.gius.pe.Shape.witness.Witness;
import it.gius.pe.algorithm.gjk.DistanceSolution;
import it.gius.pe.algorithm.gjk.IGjkAlgorithmShape;
import it.gius.pe.algorithm.gjk.Simplex;
import it.gius.pe.algorithm.gjk.SimplexSolution;

public abstract class GjkSingleShape implements IGjkAlgorithmShape{

	protected Simplex simplex;
	protected Vec2 currentSupportPoint;

	public GjkSingleShape() {
		currentSupportPoint = new Vec2();
		simplex = new Simplex(Simplex.LARGER_2D_SIMPLEX);
	}

	@Override
	public Simplex updateSimplex(SimplexSolution simpSol, Vec2 newSupportPoint,Witness witness) {

		boolean supportAlreadyIn = false;

		currentSupportPoint.set(newSupportPoint);
		//supportPoint(d, currentSupportPoint);

		for(int i =0; i< simpSol.numVerticesUsed;i++)
		{
			simplex.vs[i].set(simpSol.verticesInSolution[i]);
			if(simplex.vs[i].equals(currentSupportPoint))
				supportAlreadyIn = true;
		}

		simplex.currentDim = simpSol.numVerticesUsed;

		if(supportAlreadyIn)
			return null;

		if(simplex.currentDim < Simplex.LARGER_2D_SIMPLEX)
		{
			simplex.vs[simpSol.numVerticesUsed].set(currentSupportPoint);
			simplex.currentDim++;
		}

		return simplex;
	}

	@Override
	public void fillSolution(Vec2 pointSol, Vec2 q, SimplexSolution npSol,
			DistanceSolution sol) {

		//point inside the poly
		if(npSol.numVerticesUsed == Simplex.LARGER_2D_SIMPLEX)
		{
			sol.p1 = q;
			sol.distance = 0;
		}
		else
		{
			sol.p1 = pointSol;
		}
		
		sol.lastSimplex = simplex;


		/*int index = 0;
				for(;index <poly.getVerticesCount();index++)
				{
					for(int j=0; j<npSol.numVerticesUsed && !npSol.equals(poly.getVertex(index)); j++);
				}

				if(index< poly.getVerticesCount())
					startPointIndex = index;
		 */
	}

}